#include"vld_imu_slam/feature_extract.h"

namespace vld_imu_slam{

FeatureExtract::FeatureExtract(vld_imu_slam::Config::Ptr p_config){
    p_config_ = p_config;
    is_get_deq_odom_.store(false);
}

void FeatureExtract::getImuPoseEstimate(deque<nav_msgs::Odometry> deq_odom){
    lock_guard<mutex> lck(deq_odom_mutex_);
    if(!deq_odom.empty()){
        deq_odom_ = deq_odom;
        is_get_deq_odom_.store(true);
    }
}

void FeatureExtract::setGetImuPoseFlag(bool flag){
    lock_guard<mutex> lck(deq_odom_mutex_);
    is_get_deq_odom_.store(flag);
}

bool FeatureExtract::getPoseFromImudata(double time,SE3& pose){
    lock_guard<mutex> lck(deq_odom_mutex_);
    if(!is_get_deq_odom_.load())
        return false;
    if(deq_odom_.empty()){
        return false;
    }else{
        int idx_back = deq_odom_.size()-1;
        for(int i=0;i<deq_odom_.size();i++){
            if(deq_odom_[i].header.stamp.toSec()>=time){
                idx_back = i;
                break;
            }
        }
        if(idx_back==deq_odom_.size()-1 ){
            if(abs(deq_odom_.back().header.stamp.toSec()-time)<0.5){
                pose = SE3(Eigen::Quaterniond(deq_odom_.back().pose.pose.orientation.w,deq_odom_.back().pose.pose.orientation.x,
                    deq_odom_.back().pose.pose.orientation.y,deq_odom_.back().pose.pose.orientation.z),
                    Eigen::Vector3d(deq_odom_.back().pose.pose.position.x,deq_odom_.back().pose.pose.position.y,deq_odom_.back().pose.pose.position.z));
                return true;
            }else{
                return false;
            }
        }
        tf::Transform tf_b,tf_f;
        tf::poseMsgToTF(deq_odom_[idx_back].pose.pose, tf_b);
        tf::poseMsgToTF(deq_odom_[idx_back-1].pose.pose, tf_f);
        double roll_b, yaw_b,pitch_b,roll_f, yaw_f,pitch_f,x_temp,y_temp,z_temp,roll_temp,pitch_temp,yaw_temp;
        tf::Matrix3x3(tf_b.getRotation()).getRPY(roll_b, pitch_b, yaw_b);
        tf::Matrix3x3(tf_f.getRotation()).getRPY(roll_f, pitch_f, yaw_f);
        double ratio = (time-deq_odom_[idx_back-1].header.stamp.toSec())/(deq_odom_[idx_back].header.stamp.toSec()-deq_odom_[idx_back-1].header.stamp.toSec());
        x_temp = tf_f.getOrigin().getX()+ratio*(tf_b.getOrigin().getX() - tf_f.getOrigin().getX());
        y_temp = tf_f.getOrigin().getY()+ratio*(tf_b.getOrigin().getY() - tf_f.getOrigin().getY());
        z_temp = tf_f.getOrigin().getZ()+ratio*(tf_b.getOrigin().getZ() - tf_f.getOrigin().getX());
        roll_temp = roll_f+ratio*(roll_b-roll_f);
        pitch_temp = pitch_f+ratio*(pitch_b-pitch_f);
        yaw_temp = yaw_f+ratio*(yaw_b-yaw_f);
        Eigen::AngleAxisd r_x(roll_temp,Eigen::Vector3d::UnitX());
        Eigen::AngleAxisd r_y(pitch_temp,Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd r_z(yaw_temp,Eigen::Vector3d::UnitZ());
        pose = SE3(r_x*r_y*r_z, Eigen::Vector3d(x_temp,y_temp,z_temp));
        return true;
    }
}

void FeatureExtract::pcHandle(const sensor_msgs::PointCloud2ConstPtr &msg){
    //judge data is ok?
    if (msg->header.stamp.toSec() < pc_last_timestamp_)
    {
        ROS_ERROR("lidar loop back, clear buffer");
        pc_last_timestamp_ = msg->header.stamp.toSec();
        return;
    }
    pc_last_timestamp_ = msg->header.stamp.toSec();

    projectPC(msg);

    vector<vector<PointInfo>> pc_array_temp(p_config_->n_scan_,vector<PointInfo>(p_config_->num_points_a_line_));
    int col_new=0;
    pc_full_.reset(new pcl::PointCloud<PointType>);
    PointType pcl_point;
    for(int i=0;i<p_config_->n_scan_;i++){
        for(int j=0;j<p_config_->num_points_a_line_;j++){
            if(pc_array_[i][j].point_type_!=EPointType::e_pt_normal_)
                continue;
            pc_array_temp[i][col_new] = pc_array_[i][j];
            col_new++;
            pcl_point.x = pc_array_[i][j].x_;
            pcl_point.y = pc_array_[i][j].y_;
            pcl_point.z = pc_array_[i][j].z_;
            pc_full_->push_back(pcl_point);
        }
        col_new = 0;
    }
    pc_array_ = pc_array_temp;

    //undistort motion distortion
    // if(imu_interg_pose_queue_.size()){

    // }

    //lego loam
    // judgeGround();
    //lego loam

    markOccludedPoints();

    featureExtract();

    samplePoint();

    ROS_DEBUG_STREAM("sample plane num: "<<pc_surface_->points.size());
    ROS_DEBUG_STREAM("sample corner num: "<<pc_conner_->points.size());

}

void FeatureExtract::projectPC(const sensor_msgs::PointCloud2ConstPtr &msg){
    pc_raw_.reset(new pcl::PointCloud<VelodynePointXYZIRT>);
    pcl::fromROSMsg(*msg, *pc_raw_);

    pc_array_ = vector<vector<PointInfo>>(p_config_->n_scan_, vector<PointInfo>(p_config_->num_points_a_line_));
    roughness_array_ = vector<vector<RoughNessIdx>>(p_config_->n_scan_, vector<RoughNessIdx>(p_config_->num_points_a_line_));
    PointInfo pt;

    int cloudSize = pc_raw_->points.size();
    for (int i = 0; i < cloudSize; ++i)
    {
        // pcl格式
        PointType thisPoint;
        thisPoint.x = pc_raw_->points[i].x;
        thisPoint.y = pc_raw_->points[i].y;
        thisPoint.z = pc_raw_->points[i].z;
        thisPoint.intensity = pc_raw_->points[i].intensity;

        // 距离检查
        float range = pointDistance(thisPoint);
        if (range < 1 || range > 1000)
            continue;

        // 扫描线检查
        int rowIdn = pc_raw_->points[i].ring;
        if (rowIdn < 0 || rowIdn >= p_config_->n_scan_)
            continue;

        float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

        // 水平扫描角度步长，例如一周扫描1800次，则两次扫描间隔角度0.2°
        static float ang_res_x = 360.0/float(p_config_->num_points_a_line_);
        int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + p_config_->num_points_a_line_/2;
        if (columnIdn >= p_config_->num_points_a_line_)
            columnIdn -= p_config_->num_points_a_line_;

        if (columnIdn < 0 || columnIdn >= p_config_->num_points_a_line_)
            continue;

        // 已经存过该点，不再处理
        if (pc_array_[rowIdn][columnIdn].point_type_ == EPointType::e_pt_normal_)
            continue;
        if(abs(pc_raw_->points[i].x)<1000&&abs(pc_raw_->points[i].y)<1000&&abs(pc_raw_->points[i].z)<1000)
            pt.point_type_ = EPointType::e_pt_normal_;
        else
            pt.point_type_ = EPointType::e_pt_nan_;

        pt.line = pc_raw_->points[i].ring;
        pt.time_stamp_ = pc_raw_->header.stamp+i*time_interval_VLD16_s_;
        pt.x_ = pc_raw_->points[i].x;
        pt.y_ = pc_raw_->points[i].y;
        pt.z_ = pc_raw_->points[i].z;
        pt.range_ = range;
        pc_array_[rowIdn][columnIdn] = pt;

    }
    
}

void FeatureExtract::featureExtract(){
    PointIdx cur_pt_idx;
    for(int i=0;i<p_config_->n_scan_;i++){
        for(int j=5;j<p_config_->num_points_a_line_-5;j++){
            if(pc_array_[i][j].point_type_!=EPointType::e_pt_normal_ || pc_array_[i][j].point_label_ != EPointLabel::e_uncertain_)
                continue;
            cur_pt_idx.i = i; cur_pt_idx.j = j;
            judgeRoughness(cur_pt_idx);
        }
    }
    // pc_array_[0][0].point_label_ = EPointLabel::e_corner_neighbour;
}

void FeatureExtract::judgeRoughness(PointIdx& p_idx){
    int num_neighbours = 0;
    double roughness = 0;
    Eigen::Vector3d roughness_v(0,0,0);
    for(int j=-1;j>-6;j--)
        judgePoint(pc_array_[p_idx.i][p_idx.j+j],roughness_v,num_neighbours);
    for(int j=1;j<6;j++)
        judgePoint(pc_array_[p_idx.i][p_idx.j+j],roughness_v,num_neighbours);
  
    roughness_v[0] = roughness_v[0] - num_neighbours*pc_array_[p_idx.i][p_idx.j].x_;
    roughness_v[1] = roughness_v[1] - num_neighbours*pc_array_[p_idx.i][p_idx.j].y_;
    roughness_v[2] = roughness_v[2] - num_neighbours*pc_array_[p_idx.i][p_idx.j].z_;
    roughness_array_[p_idx.i][p_idx.j].roughness = roughness_v.norm()*roughness_v.norm();
    roughness_array_[p_idx.i][p_idx.j].idx = p_idx;
}

void FeatureExtract::judgePoint(PointInfo& pt, Eigen::Vector3d& r_v, int& num){
    if(pt.point_type_==EPointType::e_pt_normal_){
        num++;
        r_v[0] = r_v[0] +pt.x_;
        r_v[1] = r_v[1] +pt.y_;
        r_v[2] = r_v[2] +pt.z_;
    }
}

void FeatureExtract::markOccludedPoints(){
    for(int i=0;i<p_config_->n_scan_;i++){
        for(int j=5;j<p_config_->num_points_a_line_-5;j++){
            if(pc_array_[i][j].point_type_!=EPointType::e_pt_normal_ )
                continue;

            if (pc_array_[i][j].range_ - pc_array_[i][j+1].range_ > 0.3){
                pc_array_[i][j-5].point_label_ = EPointLabel::e_corner_neighbour;
                pc_array_[i][j - 4].point_label_ =  EPointLabel::e_corner_neighbour;
                pc_array_[i][j- 3].point_label_ =  EPointLabel::e_corner_neighbour;
                pc_array_[i][j - 2].point_label_ =  EPointLabel::e_corner_neighbour;
                pc_array_[i][j- 1].point_label_ =  EPointLabel::e_corner_neighbour;
                pc_array_[i][j].point_label_ =  EPointLabel::e_corner_neighbour;
            }else if (pc_array_[i][j+1].range_ - pc_array_[i][j].range_ > 0.3){
                pc_array_[i][j+1].point_label_ = EPointLabel::e_corner_neighbour;
                pc_array_[i][j +2].point_label_ =  EPointLabel::e_corner_neighbour;
                pc_array_[i][j +3].point_label_ =  EPointLabel::e_corner_neighbour;
                pc_array_[i][j +4].point_label_ =  EPointLabel::e_corner_neighbour;
                pc_array_[i][j+5].point_label_ =  EPointLabel::e_corner_neighbour;
                pc_array_[i][j+6].point_label_ =  EPointLabel::e_corner_neighbour;
            }
            // 用前后相邻点判断当前点所在平面是否与激光束方向平行
            float diff1 = std::abs(float(pc_array_[i][j-1].range_ - pc_array_[i][j].range_));
            float diff2 = std::abs(float(pc_array_[i][j+1].range_ - pc_array_[i][j].range_));

            // 平行则标记一下
            if (diff1 > 0.02 * pc_array_[i][j].range_ && diff2 > 0.02 * pc_array_[i][j].range_)
                pc_array_[i][j].point_label_ = EPointLabel::e_corner_neighbour;
        }
    }
}


//lio sam
void FeatureExtract::samplePoint(){
    int gap = gap = p_config_->num_points_a_line_/6;
    pc_conner_.reset(new pcl::PointCloud<PointType>);
    pc_surface_.reset(new pcl::PointCloud<PointType>);
    for(int row=0;row<p_config_->n_scan_;row++){
        for(int interval_idx = 0; interval_idx<6;interval_idx++){
            int p_left = interval_idx*gap;
            int p_right = interval_idx*gap+gap-1;
            sort(roughness_array_[row].begin()+p_left, roughness_array_[row].begin()+p_right+1, by_value()); //排序是不会包括p_right的

            int largestPickedNum = 0;
            for (int k = p_right; k >= p_left; k--) {
                // 每次ind的值就是等于k??? 有什么意义?
                // 因为上面对cloudSmoothness进行了一次从小到大排序，所以ind不一定等于k了
                PointIdx idx = roughness_array_[row][k].idx;
                if (pc_array_[idx.i][idx.j].point_label_==EPointLabel::e_uncertain_ && pc_array_[idx.i][idx.j].point_type_ == EPointType::e_pt_normal_ &&
                    roughness_array_[row][k].roughness > p_config_->conner_roughness_threshold_) {
                
                    largestPickedNum++;
                    if (largestPickedNum <= 20) {
                        addCornerPoint(pc_array_[idx.i][idx.j],pc_conner_);
                    }  else {
                        break;
                    }

                    for (int l = 1; l <= 5; l++) {
                        // 从ind+l开始后面5个点，每个点index之间的差值，
                        // 确保columnDiff<=10,然后标记为我们需要的点
                        if(idx.j+l<p_config_->num_points_a_line_)
                            pc_array_[idx.i][idx.j+l].point_label_ = EPointLabel::e_corner_neighbour;
                    }
                    for (int l = -1; l >= -5; l--) {
                        // 从ind+l开始前面五个点，计算差值然后标记
                        if(idx.j+l>=0)
                            pc_array_[idx.i][idx.j+l].point_label_ = EPointLabel::e_corner_neighbour;
                    }
                }
            }

            for (int k = p_left; k <= p_right; k++) {
                // 每次ind的值就是等于k??? 有什么意义?
                // 因为上面对cloudSmoothness进行了一次从小到大排序，所以ind不一定等于k了
                PointIdx idx = roughness_array_[row][k].idx;
                // if (pc_array_[idx.i][idx.j].point_label_==EPointLabel::e_uncertain_ &&
                //     roughness_array_[row][k].roughness < p_config_->plane_roughness_threshold_)
                if (pc_array_[idx.i][idx.j].point_label_==EPointLabel::e_uncertain_ && pc_array_[idx.i][idx.j].point_type_ == EPointType::e_pt_normal_)
                {
                
                    addPlanePoint(pc_array_[idx.i][idx.j],pc_surface_);

                    for (int l = 1; l <= 5; l++) {
                        // 从ind+l开始后面5个点，每个点index之间的差值，
                        // 确保columnDiff<=10,然后标记为我们需要的点
                        if(idx.j+l<p_config_->num_points_a_line_)
                            pc_array_[idx.i][idx.j+l].point_label_ = EPointLabel::e_corner_neighbour;
                    }
                    for (int l = -1; l >= -5; l--) {
                        // 从ind+l开始前面五个点，计算差值然后标记
                        if(idx.j+l>=0)
                            pc_array_[idx.i][idx.j+l].point_label_ = EPointLabel::e_corner_neighbour;
                    }
                }
            }
        }      
    }
    // pcDownSample(pc_conner_,0.2);
    // pcDownSample(pc_surface_,0.4);

}

void FeatureExtract::addCornerPoint(PointInfo& pt,pcl::PointCloud<PointType>::Ptr pc){
    PointType pt_ground;
    pt_ground.x = pt.x_;
    pt_ground.y = pt.y_;
    pt_ground.z = pt.z_;
    pc->push_back(pt_ground);
    pt.point_label_ = EPointLabel::e_corner_;
}

void FeatureExtract::addPlanePoint(PointInfo& pt,pcl::PointCloud<PointType>::Ptr pc){
    PointType pt_ground;
    pt_ground.x = pt.x_;
    pt_ground.y = pt.y_;
    pt_ground.z = pt.z_;
    pc->push_back(pt_ground);
    pt.point_label_ = EPointLabel::e_plane_;
}



//lego loam
// void FeatureExtract::samplePoint(){
//     int gap = p_config_->num_points_a_line_/p_config_->ground_sample_points_num_;
//     pc_groundPt_sample_.reset(new pcl::PointCloud<PointType>);
//     pc_cornerPt_sample_.reset(new pcl::PointCloud<PointType>);
//     for(int row=0;row<p_config_->n_scan_-9;row++){
//         for(int interval_idx = 0; interval_idx<p_config_->ground_sample_points_num_;interval_idx++){
//             int p_left = interval_idx*gap+gap/2-1;
//             int p_right = interval_idx*gap+gap/2;
//             bool find_g = false; 
//             int count =0;
//             while(true){
//                 if(!find_g)
//                     find_g = addGroundPoint(pc_array_[row][p_left]);
//                 if(!find_g)
//                     find_g = addGroundPoint(pc_array_[row][p_right]);
//                 if(!find_g){
//                     if(p_left>interval_idx*gap)
//                         p_left--;
//                     if(p_right<(interval_idx+1)*gap-1)
//                         p_right++;
//                 }
//                 if(find_g)
//                     break;
//                 if(p_left==interval_idx*gap&&p_right==(interval_idx+1)*gap-1)
//                     break;
//             }
//         }
//     }

//     pc_conner_.reset(new pcl::PointCloud<PointType>());
//     gap = p_config_->num_points_a_line_/6;
//     for(int row=0;row<p_config_->n_scan_;row++){
//         for(int interval_idx = 0; interval_idx<6;interval_idx++){
//             int p_left = interval_idx*gap;
//             int p_right = interval_idx*gap+gap-1;
//             sort(roughness_array_[row].begin()+p_left, roughness_array_[row].begin()+p_right+1, by_value()); //排序是不会包括p_right的

//             int largestPickedNum = 0;
//             for (int k = p_right; k >= p_left; k--) {
//                 // 每次ind的值就是等于k??? 有什么意义?
//                 // 因为上面对cloudSmoothness进行了一次从小到大排序，所以ind不一定等于k了
//                 PointIdx idx = roughness_array_[row][k].idx;
//                 if (pc_array_[idx.i][idx.j].point_label_==EPointLabel::e_uncertain_ &&
//                     roughness_array_[row][k].roughness > p_config_->conner_roughness_threshold_) {
                
//                     largestPickedNum++;
//                     if (largestPickedNum <= 2) {
//                         // 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列，
//                         // 所以这边只要选择最后两个放进队列即可
//                         // cornerPointsSharp标记为2
//                         addCornerPoint(pc_array_[idx.i][idx.j],pc_cornerPt_sample_);
//                         addCornerPoint(pc_array_[idx.i][idx.j],pc_conner_);
//                     } else if (largestPickedNum <= 100) {
//                         // 塞20个点到cornerPointsLessSharp中去
//                         // cornerPointsLessSharp标记为1
//                         addCornerPoint(pc_array_[idx.i][idx.j],pc_conner_);
//                     } else {
//                         break;
//                     }

//                     for (int l = 1; l <= 5; l++) {
//                         // 从ind+l开始后面5个点，每个点index之间的差值，
//                         // 确保columnDiff<=10,然后标记为我们需要的点
//                         if(idx.j+l<=p_right)
//                             pc_array_[idx.i][idx.j+l].point_label_ = EPointLabel::e_corner_neighbour;
//                     }
//                     for (int l = -1; l >= -5; l--) {
//                         // 从ind+l开始前面五个点，计算差值然后标记
//                         if(idx.j+l>=p_left)
//                             pc_array_[idx.i][idx.j+l].point_label_ = EPointLabel::e_corner_neighbour;
//                     }
//                 }
//             }
//         }
//     }

// }

// bool FeatureExtract::addGroundPoint(PointInfo& pt){
//     if(pt.point_label_==EPointLabel::e_ground_){
//         PointType pt_ground;
//         pt_ground.x = pt.x_;
//         pt_ground.y = pt.y_;
//         pt_ground.z = pt.z_;
//         pc_groundPt_sample_->push_back(pt_ground);
//         return true;
//     }else{
//         return false;
//     }
// }

// void FeatureExtract::judgeGround(){
//     pc_ground_.reset(new pcl::PointCloud<PointType>);
//     double angle,diffX,diffY,diffZ;
//     PointType pt;
//     for(int j=0;j<p_config_->num_points_a_line_-1;j++){
//         for(int i=0;i<p_config_->n_scan_-9;i++){
//             if(pc_array_[i][j].point_type_ == EPointType::e_pt_normal_&&
//                 pc_array_[i+1][j].point_type_ == EPointType::e_pt_normal_&&
//                 (pc_array_[i][j].point_label_ == EPointLabel::e_uncertain_||
//                 pc_array_[i+1][j].point_label_ == EPointLabel::e_uncertain_)){
//                 diffX = pc_array_[i+1][j].x_ - pc_array_[i][j].x_;
//                 diffY = pc_array_[i+1][j].y_ - pc_array_[i][j].y_;
//                 diffZ = pc_array_[i+1][j].z_ - pc_array_[i][j].z_;
//                 angle = atan2(diffZ, sqrt(diffX*diffX  + diffY*diffY) ) * 180 / M_PI;
//                 if (abs(angle) <= 10){
//                     pc_array_[i][j].point_label_ = EPointLabel::e_ground_;
//                     pt.x = pc_array_[i][j].x_; pt.y = pc_array_[i][j].y_; pt.z = pc_array_[i][j].z_;
//                     pc_ground_->push_back(pt);
//                 }
//             }
//         }
//     }
// }

// void FeatureExtract::addCornerPoint(PointInfo& pt,pcl::PointCloud<PointType>::Ptr pc){
//     PointType pt_ground;
//     pt_ground.x = pt.x_;
//     pt_ground.y = pt.y_;
//     pt_ground.z = pt.z_;
//     pc->push_back(pt_ground);
//     pt.point_label_ = EPointLabel::e_corner_;
// }

}
